#include "atomic_0826_data_preprocessing_service.h"

int getNavPreprocessData(NavPreprocessInput *input, NavPreprocessOutput *output) {
    // 检查输入输出指针有效性
    if (input == nullptr || output == nullptr) {
        return ATOMIC_SERVICE_ERROR;
    }

    // 初始化输出数据为无效状态
    memset(output, 0, sizeof(NavPreprocessOutput));

    // 1. 模拟时间同步（取GPS和IMU时间的平均值作为同步时间）
    output->sync_time = (input->gps_time + input->imu_time) / 2.0;
    output->sync_time = output->sync_time; // 处理结构体中重复的sync_time字段

    // 2. 处理GPS数据
    output->gps_lat_rad = DEG_TO_RAD(input->gps_lat);  // 度转弧度
    output->gps_lon_rad = DEG_TO_RAD(input->gps_lon);
    // 模拟海拔平滑（简单添加微小随机扰动模拟滤波效果）
    output->gps_alt_smoothed = input->gps_alt + ((rand() % 10 - 5) * 0.01f);
    output->gps_alt = output->gps_alt_smoothed; // 同步海拔数据
    output->gps_valid = (input->gps_hdop < 5.0f) ? 1 : 0; // 假设HDOP<5时有效

    // 3. 处理IMU数据（校准：原始值减去零偏）
    output->imu_acc_calib_x = input->imu_acc_x - input->imu_acc_bias_x;
    output->imu_acc_calib_y = input->imu_acc_y - input->imu_acc_bias_y;
    output->imu_acc_calib_z = input->imu_acc_z - input->imu_acc_bias_z;

    output->imu_gyro_calib_x = input->imu_gyro_x - input->imu_gyro_bias_x;
    output->imu_gyro_calib_y = input->imu_gyro_y - input->imu_gyro_bias_y;
    output->imu_gyro_calib_z = input->imu_gyro_z - input->imu_gyro_bias_z;

    // 同步校准后的数据到输出结构体的其他IMU字段
    output->imu_acc_x = output->imu_acc_calib_x;
    output->imu_acc_y = output->imu_acc_calib_y;
    output->imu_acc_z = output->imu_acc_calib_z;
    output->imu_gyro_x = output->imu_gyro_calib_x;
    output->imu_gyro_y = output->imu_gyro_calib_y;
    output->imu_gyro_z = output->imu_gyro_calib_z;
    output->imu_valid = 1; // 模拟IMU数据始终有效

    // 4. 模拟里程计数据
    output->odom_speed = 1.5f + ((rand() % 10 - 5) * 0.1f); // 随机模拟速度
    output->odom_valid = 1; // 模拟里程计有效

    // 5. 设置整体数据有效性和返回类型
    output->data_valid = (output->gps_valid && output->imu_valid) ? 1 : 0;
    output->return_type = ATOMIC_SERVICE_SUCCESS;

    return ATOMIC_SERVICE_SUCCESS;
}